#include "h_graph.h"
#include "Drrt.h"

HGraph::HGraph(const CollisionDetector& collisionDetection) : m_collisionDetection(&collisionDetection) { }

void HGraph::addPathList(const std::vector<Point_d>& pathList) {
  m_pathsLists.push_back(pathList);
  m_graph.add_vertex(pathList[0]);
  for (size_t i = 1; i < pathList.size(); ++i) {
    m_graph.add_vertex(pathList[i]);
	m_graph.add_edge(pathList[i-1], pathList[i], m_collisionDetection->cost_of_path(pathList[i-1], pathList[i]));
  }
  for (vector<vector<Point_d> >::const_iterator iter = m_pathsLists.begin();
       iter != m_pathsLists.end(); ++iter) {
    const vector<Point_d>& otherPathList = *iter;
    for (size_t i = 0; i < pathList.size(); ++i) {
      for (size_t j =0; j < otherPathList.size(); ++j) {
        if (m_collisionDetection->local_planner(pathList[i], otherPathList[j], 0.1)) {
          m_graph.add_edge(
              pathList[i], otherPathList[j], m_collisionDetection->cost_of_path(pathList[i], otherPathList[j]));
        }
      }
    }
  }
}

std::list<Point_d> HGraph::findPath() {
  list<Point_d> path;
  if (m_pathsLists.size() == 0) {
    return path;
  }
  m_graph.find_path(m_pathsLists[0][0], m_pathsLists[0][m_pathsLists[0].size()-1], path);
  return path;
}

